<pre class="code">
<span class="srcline"><span class="lineno"><a href="47,1" id="srcline1">  1</a></span><span class="line"><span class="comment">%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,2" id="srcline2">  2</a></span><span class="line"><span class="comment">% RPI-MATLAB-Simulator</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,3" id="srcline3">  3</a></span><span class="line"><span class="comment">% http://code.google.com/p/rpi-matlab-simulator/</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,4" id="srcline4">  4</a></span><span class="line"><span class="comment">%</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,5" id="srcline5">  5</a></span><span class="line"><span class="comment">% This function updates bodies positions and velocities of simulator sim to</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,6" id="srcline6">  6</a></span><span class="line"><span class="comment">% correct for joint constraint violations. </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,7" id="srcline7">  7</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,8" id="srcline8">  8</a></span><span class="line"><span class="keyword">function</span> <span class="var type1" id="S2T6U3">sim</span> = joint_correction( <span class="mxinfo" id="T6:U2"><span class="mxinfo" id="T6:U3"><span class="var type1" id="S2T6U6">sim</span></span></span> )</span></span>
<span class="srcline"><span class="lineno"><a href="47,9" id="srcline9">  9</a></span><span class="line"><span class="comment">%#codegen</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,10" id="srcline10"> 10</a></span><span class="line">    <span class="mxinfo" id="T1:U5"><span class="var type1" id="S3T1U9">epsPosition</span> = <span class="mxinfo" id="T1:U7"><span class="mxinfo" id="T1:U8">10</span>^-5</span></span>;                  <span class="comment">% Joint position error epsilon  </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,11" id="srcline11"> 11</a></span><span class="line">    <span class="mxinfo" id="T1:U9"><span class="var type1" id="S4T1U16">epsVelocity</span> = <span class="mxinfo" id="T1:U11"><span class="mxinfo" id="T1:U12">10</span>^-5</span></span>;                  <span class="comment">% Joint velocity error epsilon </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,12" id="srcline12"> 12</a></span><span class="line">    <span class="mxinfo" id="T1:U13"><span class="var type1" id="S5T1U23">maxIters</span> = <span class="mxinfo" id="T1:U15">25</span></span>;            </span></span>
<span class="srcline"><span class="lineno"><a href="47,13" id="srcline13"> 13</a></span><span class="line">    <span class="mxinfo" id="T1:U16"><span class="var type1" id="S6T1U27">nj</span> = <span class="mxinfo" id="T1:U18">length(<span class="var type1" id="S2T6U31">sim</span>.joints)</span></span>;              <span class="comment">% Number of joints </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,14" id="srcline14"> 14</a></span><span class="line">    <span class="comment">%nb = sim.num_activeJointBodies;       % Number of bodies involved</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,15" id="srcline15"> 15</a></span><span class="line">    <span class="comment">%njc = sim.num_jointConstraints;       % Number of joint constraints</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,16" id="srcline16"> 16</a></span><span class="line">    <span class="mxinfo" id="T1:U20"><span class="var type1" id="S8T1U35">h</span> = <span class="mxinfo" id="T1:U22"><span class="var type1" id="S2T6U37">sim</span>.h</span></span>;                            <span class="comment">% Simulation time-step </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,17" id="srcline17"> 17</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,18" id="srcline18"> 18</a></span><span class="line">    <span class="keyword">if</span> <span class="mxinfo" id="T4:U24"><span class="var type1" id="S2T6U42">sim</span>.jointCorrection</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,19" id="srcline19"> 19</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,20" id="srcline20"> 20</a></span><span class="line">      <span class="comment">%% For every joint, record values BEFORE update is applied, i.e. at time t0</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,21" id="srcline21"> 21</a></span><span class="line">      <span class="keyword">for</span> <span class="mxinfo" id="T1:U26"><span class="var type1" id="S9T1U46">j</span></span>=1:<span class="var type1" id="S6T1U49">nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,22" id="srcline22"> 22</a></span><span class="line">         <span class="var type0" id="S10T0U52">J</span> = <span class="var type2" id="S2T0U55">sim</span>.joints(<span class="var type2" id="S9T0U57">j</span>);</span></span>
<span class="srcline"><span class="lineno"><a href="47,23" id="srcline23"> 23</a></span><span class="line">         [<span class="var type0" id="S11T0U61">Gb</span>, ~, ~] = joint_getCorrectionValues(<span class="var type2" id="S2T0U66">sim</span>,<span class="var type2" id="S9T0U67">j</span>); <span class="comment">% J.getJointCorrectionValues();</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,24" id="srcline24"> 24</a></span><span class="line">         <span class="var type0" id="S10T0U72">J</span>.bender.Gb_t0 = <span class="var type0" id="S11T0U75">Gb</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,25" id="srcline25"> 25</a></span><span class="line">         <span class="var type0" id="S10T0U80">J</span>.bender.Minv = inv(<span class="var type0" id="S10T0U87">J</span>.getMassInertiaMatrix());</span></span>
<span class="srcline"><span class="lineno"><a href="47,26" id="srcline26"> 26</a></span><span class="line">         <span class="var type0" id="S10T0U93">J</span>.bender.MinvGb0 = <span class="var type0" id="S10T0U99">J</span>.getMassInertiaMatrix() \ <span class="var type0" id="S11T0U101">Gb</span>;  </span></span>
<span class="srcline"><span class="lineno"><a href="47,27" id="srcline27"> 27</a></span><span class="line">         <span class="var type0" id="S10T0U105">J</span>.update(); </span></span>
<span class="srcline"><span class="lineno"><a href="47,28" id="srcline28"> 28</a></span><span class="line">      <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,29" id="srcline29"> 29</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,30" id="srcline30"> 30</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,31" id="srcline31"> 31</a></span><span class="line">      <span class="comment">%% Position correction </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,32" id="srcline32"> 32</a></span><span class="line">      <span class="mxinfo" id="T1:U29"><span class="var type1" id="S14T1U109">PiterCount</span> = <span class="mxinfo" id="T1:U31">0</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,33" id="srcline33"> 33</a></span><span class="line">      <span class="mxinfo" id="T1:U32"><span class="var type1" id="S15T1U113">Cmax</span> = <span class="mxinfo" id="T1:U34">inf</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,34" id="srcline34"> 34</a></span><span class="line">      <span class="mxinfo" id="T1:U35"><span class="var type1" id="S17T1U118">Piter</span> = <span class="mxinfo" id="T1:U37">0</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,35" id="srcline35"> 35</a></span><span class="line">      <span class="mxinfo" id="T4:U38"><span class="var type1" id="S18T4U122">Pcorrected</span> = <span class="mxinfo" id="T4:U40">false</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,36" id="srcline36"> 36</a></span><span class="line">      <span class="keyword">while</span> <span class="mxinfo" id="T4:U41"><span class="var type1" id="S17T1U128">Piter</span> &lt; <span class="mxinfo" id="T1:U43"><span class="var type1" id="S5T1U129">maxIters</span></span></span> &amp;&amp; <span class="mxinfo" id="T4:U45"><span class="var type1" id="S15T1U131">Cmax</span> &gt; <span class="mxinfo" id="T1:U47"><span class="var type1" id="S3T1U132">epsPosition</span></span></span></span></span>
<span class="srcline"><span class="lineno"><a href="47,37" id="srcline37"> 37</a></span><span class="line">          <span class="mxinfo" id="T1:U49"><span class="var type1" id="S17T1U135">Piter</span> = <span class="mxinfo" id="T1:U51"><span class="var type1" id="S17T1U137">Piter</span>+<span class="mxinfo" id="T1:U53">1</span></span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,38" id="srcline38"> 38</a></span><span class="line">          <span class="mxinfo" id="T1:U54"><span class="var type1" id="S15T1U141">Cmax</span> = <span class="mxinfo" id="T1:U56">0</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,39" id="srcline39"> 39</a></span><span class="line">          <span class="mxinfo" id="T1:U57"><span class="var type1" id="S20T1U145">totalPerr</span> = <span class="mxinfo" id="T1:U59">0</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,40" id="srcline40"> 40</a></span><span class="line">          <span class="keyword">for</span> <span class="mxinfo" id="T1:U60"><span class="var type1" id="S9T1U149">j</span></span>=1:<span class="var type1" id="S6T1U152">nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,41" id="srcline41"> 41</a></span><span class="line">                <span class="var type0" id="S10T0U155">J</span> = <span class="var type2" id="S2T0U158">sim</span>.joints(<span class="var type2" id="S9T0U160">j</span>); </span></span>
<span class="srcline"><span class="lineno"><a href="47,42" id="srcline42"> 42</a></span><span class="line">                <span class="var type0" id="S10T0U164">J</span>.update(); </span></span>
<span class="srcline"><span class="lineno"><a href="47,43" id="srcline43"> 43</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,44" id="srcline44"> 44</a></span><span class="line">                <span class="var type0" id="S21T0U168">b1</span> = <span class="var type0" id="S10T0U170">J</span>.body1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,45" id="srcline45"> 45</a></span><span class="line">                <span class="var type0" id="S22T0U174">b2</span> = <span class="var type0" id="S10T0U176">J</span>.body2; </span></span>
<span class="srcline"><span class="lineno"><a href="47,46" id="srcline46"> 46</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,47" id="srcline47"> 47</a></span><span class="line">                <span class="var type0" id="S23T0U180">Gb_t0</span> = <span class="var type0" id="S10T0U183">J</span>.bender.Gb_t0; </span></span>
<span class="srcline"><span class="lineno"><a href="47,48" id="srcline48"> 48</a></span><span class="line">                <span class="var type0" id="S24T0U188">Minv</span> = <span class="var type0" id="S10T0U191">J</span>.bender.Minv; </span></span>
<span class="srcline"><span class="lineno"><a href="47,49" id="srcline49"> 49</a></span><span class="line">                <span class="var type0" id="S25T0U196">MinvGb0</span> = <span class="var type0" id="S10T0U199">J</span>.bender.MinvGb0; </span></span>
<span class="srcline"><span class="lineno"><a href="47,50" id="srcline50"> 50</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,51" id="srcline51"> 51</a></span><span class="line">                <span class="comment">% Calculate constraint error</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,52" id="srcline52"> 52</a></span><span class="line">                [~, <span class="var type0" id="S26T0U206">C_t1</span>, <span class="var type0" id="S27T0U207">Cdot</span>] = <span class="var type0" id="S10T0U210">J</span>.getJointCorrectionValues();</span></span>
<span class="srcline"><span class="lineno"><a href="47,53" id="srcline53"> 53</a></span><span class="line">                <span class="keyword">if</span> norm(<span class="var type0" id="S26T0U217">C_t1</span>) &gt; <span class="var type2" id="S15T0U218">Cmax</span>, <span class="var type2" id="S15T0U221">Cmax</span> = norm(<span class="var type0" id="S26T0U224">C_t1</span>); <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,54" id="srcline54"> 54</a></span><span class="line">                <span class="keyword">if</span> norm(<span class="var type0" id="S26T0U230">C_t1</span>) &gt; <span class="var type2" id="S3T0U231">epsPosition</span>, <span class="var type2" id="S18T0U234">Pcorrected</span> = true; <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,55" id="srcline55"> 55</a></span><span class="line">                </span></span>
<span class="srcline"><span class="lineno"><a href="47,56" id="srcline56"> 56</a></span><span class="line">                <span class="var type0" id="S30T0U239">pi</span> = 0; </span></span>
<span class="srcline"><span class="lineno"><a href="47,57" id="srcline57"> 57</a></span><span class="line">                        <span class="var type0" id="S31T0U243">JERR</span> = [];</span></span>
<span class="srcline"><span class="lineno"><a href="47,58" id="srcline58"> 58</a></span><span class="line">                <span class="keyword">while</span> norm(<span class="var type0" id="S26T0U251">C_t1</span>) &gt; <span class="var type2" id="S3T0U252">epsPosition</span> &amp;&amp; <span class="var type0" id="S30T0U254">pi</span> &lt; 50</span></span>
<span class="srcline"><span class="lineno"><a href="47,59" id="srcline59"> 59</a></span><span class="line">                    <span class="var type2" id="S14T0U258">PiterCount</span> = <span class="var type2" id="S14T0U260">PiterCount</span> + 1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,60" id="srcline60"> 60</a></span><span class="line">                    <span class="var type0" id="S30T0U264">pi</span> = <span class="var type0" id="S30T0U266">pi</span>+1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,61" id="srcline61"> 61</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,62" id="srcline62"> 62</a></span><span class="line">                    <span class="comment">%dp = -inv(Gb_t0' * MinvGb0) * ((C_t1/h) + Cdot);  % Cdot seems to cause trouble </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,63" id="srcline63"> 63</a></span><span class="line">                    <span class="var type0" id="S32T0U270">dp</span> = -inv(<span class="var type0" id="S23T0U277">Gb_t0</span>' * <span class="var type0" id="S25T0U278">MinvGb0</span>) * (<span class="var type0" id="S26T0U281">C_t1</span>/<span class="var type2" id="S8T0U282">h</span>); <span class="comment">% + Gb_t0'*J.Fext();</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,64" id="srcline64"> 64</a></span><span class="line">                    <span class="var type0" id="S33T0U285">dnu</span> = <span class="var type0" id="S25T0U287">MinvGb0</span> * <span class="var type0" id="S32T0U288">dp</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,65" id="srcline65"> 65</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,66" id="srcline66"> 66</a></span><span class="line">                    <span class="comment">% Update positions &amp; velocities</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,67" id="srcline67"> 67</a></span><span class="line">                    <span class="keyword">if</span> <span class="var type0" id="S21T0U292">b1</span>.static</span></span>
<span class="srcline"><span class="lineno"><a href="47,68" id="srcline68"> 68</a></span><span class="line">                        <span class="var type0" id="S22T0U297">b2</span>.u = <span class="var type0" id="S22T0U301">b2</span>.u + <span class="var type0" id="S33T0U305">dnu</span>(1:3) * <span class="var type2" id="S8T0U309">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,69" id="srcline69"> 69</a></span><span class="line">                        <span class="var type0" id="S22T0U313">b2</span>.nu = <span class="var type0" id="S22T0U317">b2</span>.nu + <span class="var type0" id="S33T0U320">dnu</span>(1:6); </span></span>
<span class="srcline"><span class="lineno"><a href="47,70" id="srcline70"> 70</a></span><span class="line">                        </span></span>
<span class="srcline"><span class="lineno"><a href="47,71" id="srcline71"> 71</a></span><span class="line">                        <span class="var type0" id="S22T0U327">b2</span>.quat = <span class="var type0" id="S22T0U331">b2</span>.quat + qtdq(<span class="var type0" id="S22T0U338">b2</span>.quat) * <span class="var type0" id="S33T0U341">dnu</span>(4:6)*<span class="var type2" id="S8T0U345">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,72" id="srcline72"> 72</a></span><span class="line">                        <span class="var type0" id="S22T0U349">b2</span>.quat = <span class="var type0" id="S22T0U353">b2</span>.quat / norm(<span class="var type0" id="S22T0U358">b2</span>.quat); </span></span>
<span class="srcline"><span class="lineno"><a href="47,73" id="srcline73"> 73</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,74" id="srcline74"> 74</a></span><span class="line">                    <span class="keyword">elseif</span> <span class="var type0" id="S22T0U362">b2</span>.static</span></span>
<span class="srcline"><span class="lineno"><a href="47,75" id="srcline75"> 75</a></span><span class="line">                        <span class="var type0" id="S21T0U367">b1</span>.u = <span class="var type0" id="S21T0U371">b1</span>.u + <span class="var type0" id="S33T0U375">dnu</span>(1:3) * <span class="var type2" id="S8T0U379">h</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,76" id="srcline76"> 76</a></span><span class="line">                        <span class="var type0" id="S21T0U383">b1</span>.nu = <span class="var type0" id="S21T0U387">b1</span>.nu + <span class="var type0" id="S33T0U390">dnu</span>(1:6);</span></span>
<span class="srcline"><span class="lineno"><a href="47,77" id="srcline77"> 77</a></span><span class="line">                        </span></span>
<span class="srcline"><span class="lineno"><a href="47,78" id="srcline78"> 78</a></span><span class="line">                        <span class="var type0" id="S21T0U397">b1</span>.quat = <span class="var type0" id="S21T0U401">b1</span>.quat + qtdq(<span class="var type0" id="S21T0U408">b1</span>.quat) * <span class="var type0" id="S33T0U411">dnu</span>(4:6)*<span class="var type2" id="S8T0U415">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,79" id="srcline79"> 79</a></span><span class="line">                        <span class="var type0" id="S21T0U419">b1</span>.quat = <span class="var type0" id="S21T0U423">b1</span>.quat / norm(<span class="var type0" id="S21T0U428">b1</span>.quat); </span></span>
<span class="srcline"><span class="lineno"><a href="47,80" id="srcline80"> 80</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,81" id="srcline81"> 81</a></span><span class="line">                    <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,82" id="srcline82"> 82</a></span><span class="line">                        <span class="var type0" id="S21T0U434">b1</span>.u = <span class="var type0" id="S21T0U438">b1</span>.u + <span class="var type0" id="S33T0U442">dnu</span>(1:3) * <span class="var type2" id="S8T0U446">h</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,83" id="srcline83"> 83</a></span><span class="line">                        <span class="var type0" id="S22T0U450">b2</span>.u = <span class="var type0" id="S22T0U454">b2</span>.u + <span class="var type0" id="S33T0U458">dnu</span>(7:9) * <span class="var type2" id="S8T0U462">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,84" id="srcline84"> 84</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,85" id="srcline85"> 85</a></span><span class="line">                        <span class="var type0" id="S21T0U466">b1</span>.nu = <span class="var type0" id="S21T0U470">b1</span>.nu + <span class="var type0" id="S33T0U473">dnu</span>(1:6);</span></span>
<span class="srcline"><span class="lineno"><a href="47,86" id="srcline86"> 86</a></span><span class="line">                        <span class="var type0" id="S22T0U480">b2</span>.nu = <span class="var type0" id="S22T0U484">b2</span>.nu + <span class="var type0" id="S33T0U487">dnu</span>(7:12); </span></span>
<span class="srcline"><span class="lineno"><a href="47,87" id="srcline87"> 87</a></span><span class="line">                        </span></span>
<span class="srcline"><span class="lineno"><a href="47,88" id="srcline88"> 88</a></span><span class="line">                        </span></span>
<span class="srcline"><span class="lineno"><a href="47,89" id="srcline89"> 89</a></span><span class="line">                        <span class="var type0" id="S21T0U494">b1</span>.quat = <span class="var type0" id="S21T0U498">b1</span>.quat + qtdq(<span class="var type0" id="S21T0U505">b1</span>.quat) * <span class="var type0" id="S33T0U508">dnu</span>(4:6)*<span class="var type2" id="S8T0U512">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,90" id="srcline90"> 90</a></span><span class="line">                        <span class="var type0" id="S21T0U516">b1</span>.quat = <span class="var type0" id="S21T0U520">b1</span>.quat / norm(<span class="var type0" id="S21T0U525">b1</span>.quat); </span></span>
<span class="srcline"><span class="lineno"><a href="47,91" id="srcline91"> 91</a></span><span class="line">                        <span class="var type0" id="S22T0U530">b2</span>.quat = <span class="var type0" id="S22T0U534">b2</span>.quat + qtdq(<span class="var type0" id="S22T0U541">b2</span>.quat) * <span class="var type0" id="S33T0U544">dnu</span>(10:12)*<span class="var type2" id="S8T0U548">h</span>;</span></span>
<span class="srcline"><span class="lineno"><a href="47,92" id="srcline92"> 92</a></span><span class="line">                        <span class="var type0" id="S22T0U552">b2</span>.quat = <span class="var type0" id="S22T0U556">b2</span>.quat / norm(<span class="var type0" id="S22T0U561">b2</span>.quat); </span></span>
<span class="srcline"><span class="lineno"><a href="47,93" id="srcline93"> 93</a></span><span class="line">                    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,94" id="srcline94"> 94</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,95" id="srcline95"> 95</a></span><span class="line">                    <span class="comment">% Recalculate constraint error</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,96" id="srcline96"> 96</a></span><span class="line">                    <span class="var type0" id="S10T0U566">J</span>.update(); </span></span>
<span class="srcline"><span class="lineno"><a href="47,97" id="srcline97"> 97</a></span><span class="line">                    [~, <span class="var type0" id="S26T0U572">C_t1</span>, ~] = <span class="var type0" id="S10T0U576">J</span>.getJointCorrectionValues();   </span></span>
<span class="srcline"><span class="lineno"><a href="47,98" id="srcline98"> 98</a></span><span class="line">                    <span class="keyword">if</span> norm(<span class="var type0" id="S26T0U583">C_t1</span>) &gt; <span class="var type2" id="S15T0U584">Cmax</span>, <span class="var type2" id="S15T0U587">Cmax</span> = norm(<span class="var type0" id="S26T0U590">C_t1</span>); <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,99" id="srcline99"> 99</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,100" id="srcline100">100</a></span><span class="line">                    <span class="comment">%C_t1'</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,101" id="srcline101">101</a></span><span class="line">                                    <span class="comment">%disp(['   ' num2str(pi) ': ' num2str(norm(C_t1)) ]);</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,102" id="srcline102">102</a></span><span class="line">                    <span class="var type0" id="S31T0U593">JERR</span> = [<span class="var type0" id="S31T0U596">JERR</span>; norm(<span class="var type0" id="S26T0U600">C_t1</span>)];</span></span>
<span class="srcline"><span class="lineno"><a href="47,103" id="srcline103">103</a></span><span class="line">                    <span class="comment">%figure(5); </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,104" id="srcline104">104</a></span><span class="line">                    <span class="comment">%plot(JERR); </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,105" id="srcline105">105</a></span><span class="line">                    </span></span>
<span class="srcline"><span class="lineno"><a href="47,106" id="srcline106">106</a></span><span class="line">                    <span class="keyword">if</span> <span class="var type2" id="S15T0U604">Cmax</span> &gt; .5 </span></span>
<span class="srcline"><span class="lineno"><a href="47,107" id="srcline107">107</a></span><span class="line">                        error(<span class="string">'Position correction is diverging.'</span>); </span></span>
<span class="srcline"><span class="lineno"><a href="47,108" id="srcline108">108</a></span><span class="line">                        <span class="comment">%return;  % Continue simulation, without correction.  </span></span></span>
<span class="srcline"><span class="lineno"><a href="47,109" id="srcline109">109</a></span><span class="line">                    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,110" id="srcline110">110</a></span><span class="line">                    <span class="comment">%disp(['  Joint ' num2str(J.jointID) ' position error (correction '  num2str(PiterCount) '): ' num2str(norm(C_t1))]);</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,111" id="srcline111">111</a></span><span class="line">                <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,112" id="srcline112">112</a></span><span class="line">                <span class="var type2" id="S20T0U612">totalPerr</span> = <span class="var type2" id="S20T0U614">totalPerr</span> + norm(<span class="var type0" id="S26T0U617">C_t1</span>); </span></span>
<span class="srcline"><span class="lineno"><a href="47,113" id="srcline113">113</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,114" id="srcline114">114</a></span><span class="line">      <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,115" id="srcline115">115</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,116" id="srcline116">116</a></span><span class="line">      <span class="keyword">if</span> <span class="var type1" id="S18T4U620">Pcorrected</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,117" id="srcline117">117</a></span><span class="line">          <span class="comment">%% Update Jacobian values between position and velocity correction</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,118" id="srcline118">118</a></span><span class="line">          <span class="keyword">for</span> <span class="var type2" id="S9T0U623">j</span>=1:<span class="var type2" id="S6T0U626">nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,119" id="srcline119">119</a></span><span class="line">             <span class="var type0" id="S10T0U629">J</span> = <span class="var type2" id="S2T0U632">sim</span>.joints(<span class="var type2" id="S9T0U634">j</span>);</span></span>
<span class="srcline"><span class="lineno"><a href="47,120" id="srcline120">120</a></span><span class="line">             <span class="var type0" id="S10T0U638">J</span>.update();          </span></span>
<span class="srcline"><span class="lineno"><a href="47,121" id="srcline121">121</a></span><span class="line">             clear <span class="string">Gb_t0</span> <span class="string">MinvGb0</span> <span class="string">J.bender.Gb_t0</span> <span class="string">J.bender.MinvGb0</span> <span class="string">C_t1</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,122" id="srcline122">122</a></span><span class="line">             [<span class="var type0" id="S37T0U650">Gb_t1</span>, ~, ~] = <span class="var type0" id="S10T0U655">J</span>.getJointCorrectionValues();  </span></span>
<span class="srcline"><span class="lineno"><a href="47,123" id="srcline123">123</a></span><span class="line">             <span class="var type0" id="S10T0U661">J</span>.bender.Gb_t1 = <span class="var type0" id="S37T0U664">Gb_t1</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,124" id="srcline124">124</a></span><span class="line">             <span class="var type0" id="S10T0U669">J</span>.bender.MinvGb_t1 = <span class="var type0" id="S10T0U675">J</span>.getMassInertiaMatrix() \ <span class="var type0" id="S37T0U677">Gb_t1</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,125" id="srcline125">125</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,126" id="srcline126">126</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,127" id="srcline127">127</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,128" id="srcline128">128</a></span><span class="line">          <span class="comment">%% Velocity correction</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,129" id="srcline129">129</a></span><span class="line">          <span class="var type0" id="S38T0U680">ViterCount</span> = 0;</span></span>
<span class="srcline"><span class="lineno"><a href="47,130" id="srcline130">130</a></span><span class="line">          <span class="var type0" id="S39T0U684">dCmax</span> = inf; </span></span>
<span class="srcline"><span class="lineno"><a href="47,131" id="srcline131">131</a></span><span class="line">          <span class="var type0" id="S40T0U689">Viter</span> = 0; </span></span>
<span class="srcline"><span class="lineno"><a href="47,132" id="srcline132">132</a></span><span class="line">          <span class="keyword">while</span> <span class="var type0" id="S40T0U694">Viter</span> &lt; <span class="var type2" id="S5T0U695">maxIters</span> &amp;&amp; <span class="var type0" id="S39T0U697">dCmax</span> &gt; <span class="var type2" id="S4T0U698">epsVelocity</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,133" id="srcline133">133</a></span><span class="line">              <span class="var type0" id="S40T0U701">Viter</span> = <span class="var type0" id="S40T0U703">Viter</span>+1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,134" id="srcline134">134</a></span><span class="line">              <span class="var type0" id="S39T0U707">dCmax</span> = 0;</span></span>
<span class="srcline"><span class="lineno"><a href="47,135" id="srcline135">135</a></span><span class="line">              <span class="keyword">for</span> <span class="var type2" id="S9T0U711">j</span>=1:<span class="var type2" id="S6T0U714">nj</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,136" id="srcline136">136</a></span><span class="line">                    <span class="var type0" id="S10T0U717">J</span> = <span class="var type2" id="S2T0U720">sim</span>.joints(<span class="var type2" id="S9T0U722">j</span>); </span></span>
<span class="srcline"><span class="lineno"><a href="47,137" id="srcline137">137</a></span><span class="line">                    <span class="var type0" id="S10T0U726">J</span>.update();  <span class="comment">% TODO: I think this is not necessary now, since only velocity is changed</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,138" id="srcline138">138</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,139" id="srcline139">139</a></span><span class="line">                    <span class="var type0" id="S21T0U730">b1</span> = <span class="var type0" id="S10T0U732">J</span>.body1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,140" id="srcline140">140</a></span><span class="line">                    <span class="var type0" id="S22T0U736">b2</span> = <span class="var type0" id="S10T0U738">J</span>.body2; </span></span>
<span class="srcline"><span class="lineno"><a href="47,141" id="srcline141">141</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,142" id="srcline142">142</a></span><span class="line">                    <span class="var type0" id="S37T0U742">Gb_t1</span> = <span class="var type0" id="S10T0U745">J</span>.bender.Gb_t1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,143" id="srcline143">143</a></span><span class="line">                    <span class="var type0" id="S24T0U750">Minv</span> = <span class="var type0" id="S10T0U753">J</span>.bender.Minv; </span></span>
<span class="srcline"><span class="lineno"><a href="47,144" id="srcline144">144</a></span><span class="line">                    <span class="var type0" id="S41T0U758">MinvGb1</span> = <span class="var type0" id="S10T0U761">J</span>.bender.MinvGb_t1; </span></span>
<span class="srcline"><span class="lineno"><a href="47,145" id="srcline145">145</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,146" id="srcline146">146</a></span><span class="line">                    [~, ~, <span class="var type0" id="S27T0U769">Cdot</span>] = <span class="var type0" id="S10T0U772">J</span>.getJointCorrectionValues();</span></span>
<span class="srcline"><span class="lineno"><a href="47,147" id="srcline147">147</a></span><span class="line">                    <span class="keyword">if</span> norm(<span class="var type0" id="S27T0U779">Cdot</span>) &gt; <span class="var type0" id="S39T0U780">dCmax</span>, <span class="var type0" id="S39T0U783">dCmax</span> = norm(<span class="var type0" id="S27T0U786">Cdot</span>); <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,148" id="srcline148">148</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,149" id="srcline149">149</a></span><span class="line">                    <span class="var type0" id="S42T0U789">vi</span> = 0;</span></span>
<span class="srcline"><span class="lineno"><a href="47,150" id="srcline150">150</a></span><span class="line">                    <span class="keyword">while</span> norm(<span class="var type0" id="S27T0U796">Cdot</span>) &gt; <span class="var type2" id="S4T0U797">epsVelocity</span> &amp;&amp; <span class="var type0" id="S42T0U799">vi</span> &lt; 20;</span></span>
<span class="srcline"><span class="lineno"><a href="47,151" id="srcline151">151</a></span><span class="line">                        <span class="var type0" id="S38T0U803">ViterCount</span> = <span class="var type0" id="S38T0U805">ViterCount</span> + 1;</span></span>
<span class="srcline"><span class="lineno"><a href="47,152" id="srcline152">152</a></span><span class="line">                        <span class="var type0" id="S42T0U809">vi</span> = <span class="var type0" id="S42T0U811">vi</span>+1; </span></span>
<span class="srcline"><span class="lineno"><a href="47,153" id="srcline153">153</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,154" id="srcline154">154</a></span><span class="line">                        <span class="var type0" id="S32T0U815">dp</span> = -inv(<span class="var type0" id="S37T0U822">Gb_t1</span>' * <span class="var type0" id="S41T0U823">MinvGb1</span>) * <span class="var type0" id="S27T0U824">Cdot</span>; </span></span>
<span class="srcline"><span class="lineno"><a href="47,155" id="srcline155">155</a></span><span class="line">                        <span class="var type0" id="S33T0U827">dnu</span> = <span class="var type0" id="S41T0U829">MinvGb1</span> * <span class="var type0" id="S32T0U830">dp</span>;                         </span></span>
<span class="srcline"><span class="lineno"><a href="47,156" id="srcline156">156</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,157" id="srcline157">157</a></span><span class="line">                        <span class="comment">% Update velocities</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,158" id="srcline158">158</a></span><span class="line">                        <span class="keyword">if</span> <span class="var type0" id="S21T0U834">b1</span>.static</span></span>
<span class="srcline"><span class="lineno"><a href="47,159" id="srcline159">159</a></span><span class="line">                            <span class="var type0" id="S22T0U839">b2</span>.nu = <span class="var type0" id="S22T0U843">b2</span>.nu + <span class="var type0" id="S33T0U846">dnu</span>(1:6); </span></span>
<span class="srcline"><span class="lineno"><a href="47,160" id="srcline160">160</a></span><span class="line">                        <span class="keyword">elseif</span> <span class="var type0" id="S22T0U852">b2</span>.static</span></span>
<span class="srcline"><span class="lineno"><a href="47,161" id="srcline161">161</a></span><span class="line">                            <span class="var type0" id="S21T0U857">b1</span>.nu = <span class="var type0" id="S21T0U861">b1</span>.nu + <span class="var type0" id="S33T0U864">dnu</span>(1:6);</span></span>
<span class="srcline"><span class="lineno"><a href="47,162" id="srcline162">162</a></span><span class="line">                        <span class="keyword">else</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,163" id="srcline163">163</a></span><span class="line">                            <span class="var type0" id="S21T0U872">b1</span>.nu = <span class="var type0" id="S21T0U876">b1</span>.nu + <span class="var type0" id="S33T0U879">dnu</span>(1:6);</span></span>
<span class="srcline"><span class="lineno"><a href="47,164" id="srcline164">164</a></span><span class="line">                            <span class="var type0" id="S22T0U886">b2</span>.nu = <span class="var type0" id="S22T0U890">b2</span>.nu + <span class="var type0" id="S33T0U893">dnu</span>(7:12); </span></span>
<span class="srcline"><span class="lineno"><a href="47,165" id="srcline165">165</a></span><span class="line">                        <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,166" id="srcline166">166</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,167" id="srcline167">167</a></span><span class="line">                        <span class="var type0" id="S10T0U900">J</span>.update(); </span></span>
<span class="srcline"><span class="lineno"><a href="47,168" id="srcline168">168</a></span><span class="line">                        [~, ~, <span class="var type0" id="S27T0U907">Cdot</span>] = <span class="var type0" id="S10T0U910">J</span>.getJointCorrectionValues(); </span></span>
<span class="srcline"><span class="lineno"><a href="47,169" id="srcline169">169</a></span><span class="line">                        <span class="keyword">if</span> norm(<span class="var type0" id="S27T0U917">Cdot</span>) &gt; <span class="var type0" id="S39T0U918">dCmax</span>, <span class="var type0" id="S39T0U921">dCmax</span> = norm(<span class="var type0" id="S27T0U924">Cdot</span>); <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,170" id="srcline170">170</a></span><span class="line">                        </span></span>
<span class="srcline"><span class="lineno"><a href="47,171" id="srcline171">171</a></span><span class="line">                        <span class="comment">%disp(['     ' num2str(vi) ': ' num2str(norm(Cdot))]);</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,172" id="srcline172">172</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,173" id="srcline173">173</a></span><span class="line">                        <span class="comment">%disp(['     Joint ' num2str(J.jointID) ' velocity error (correction '  num2str(PiterCount) '): ' num2str(norm(Cdot))]);</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,174" id="srcline174">174</a></span><span class="line">                    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,175" id="srcline175">175</a></span><span class="line">              <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,176" id="srcline176">176</a></span><span class="line">          <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,177" id="srcline177">177</a></span><span class="line">      <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,178" id="srcline178">178</a></span><span class="line">      <span class="comment">%disp(['Piters (' num2str(Cmax) '): ' num2str(PiterCount) ',  Viters (' num2str(dCmax) '): ' num2str(ViterCount)]);</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,179" id="srcline179">179</a></span><span class="line">  </span></span>
<span class="srcline"><span class="lineno"><a href="47,180" id="srcline180">180</a></span><span class="line">    <span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,181" id="srcline181">181</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="47,182" id="srcline182">182</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="47,183" id="srcline183">183</a></span><span class="line"></span></span>
</pre>
